#include<ros/ros.h>
#include<geometry_msgs/Twist.h>
#include<serial/serial.h>


using namespace std;
#define TX_MAX 13
#define PI 3.1415926

uint8_t txd[TX_MAX] = {0x5A, 0xA5, 0x0D, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
std::string port;
serial::Serial ser;

class chassis
{
 //私有属性   
private:
    /* data */
    void callback(const geometry_msgs::Twist::ConstPtr & twist);
    ros::Subscriber sub;
    ros::Publisher pub;
    ros::NodeHandle n;
    geometry_msgs::Twist twist;

//公共属性
public:
    chassis();

};
//类的构造函数
chassis::chassis(/* args */)
{
    sub = n.subscribe<geometry_msgs::Twist>("/raw_cmd_vel",100,&chassis::callback,this);
    n.getParam("chassis_driver/port",port);
    try
    {
        /* code */
        ser.setPort(port.c_str());
        ser.setBaudrate(115200);
        serial::Timeout to =serial::Timeout::simpleTimeout(5);
        ser.setTimeout(to);
        ser.open();
    }
    catch(const std::exception& e)
    {
        std::cerr << e.what() << '\n';
        printf("Unable to open port!!!");
    }
    while(ros::ok())
    {
        ros::spinOnce();
        ros::Rate loop_rate(10);
        loop_rate.sleep();
    }
    if(ser.isOpen())
    {
        ser.close();
    }
}

void chassis::callback(const geometry_msgs::Twist::ConstPtr & twist)
{
    float x_min = (twist->linear.x)*60;
    float z_min = (twist->angular.z)*60;
    float vel = z_min*0.445/2;
    uint32_t v =((x_min+vel)/(0.165*PI));
    uint32_t r =-((x_min-vel)/(0.165*PI));
    printf("linear.x:[%f],angular.z:[%f]",twist->linear.x,twist->angular.z);
    txd[0] = 0x5A;
    txd[1] = 0xA5;
    txd[2] = 0x0D;
    txd[3] = 0x02;


    txd[4] = (uint8_t)r;
    txd[5] = (uint8_t)(r>>8);
    txd[6] = (uint8_t)(r>>16);
    txd[7] = (uint8_t)(r>>24);

    txd[8] = (uint8_t)v;
    txd[9] = (uint8_t)(v>>8);
    txd[10] = (uint8_t)(v>>16);
    txd[11] = (uint8_t)(v>>24);

    int cs;
    cs = (txd[0]+txd[1]+txd[2]+txd[3]+txd[4]+txd[5]+txd[6]+txd[7]+txd[8]+txd[9]+txd[10]+txd[11]);
    txd[12] = cs;
    ser.write(txd,TX_MAX);
}
//主函数
int main(int argc,char **argv)
{
    ros::init(argc,argv,"chassis");
    chassis chassislog;

}
